package com.dizner.androidcartest;

import android.os.SystemClock;
import android.support.annotation.IntDef;
import android.support.annotation.IntRange;
import android.support.annotation.Size;
import android.support.annotation.VisibleForTesting;

import com.google.android.things.pio.I2cDevice;
import com.google.android.things.pio.PeripheralManager;
import com.google.android.things.pio.Pwm;

import java.io.IOException;
import java.util.concurrent.TimeUnit;

public class MotorHAT implements AutoCloseable {


    public static final int DEFAULT_I2C_ADDRESS = 0x60;

    private static final int MAX_DC_MOTORS = 4;

    /**
     * 停止转动.
     */
    public static final int MOTOR_STATE_RELEASE = 0;
    /**
     * 顺时针转动.
     */
    public static final int MOTOR_STATE_CW = 1;
    /**
     * 逆时针转动.
     */
    public static final int MOTOR_STATE_CCW = 2;
    private int def_freq = 1600;//默认频率

    @IntDef({MOTOR_STATE_RELEASE, MOTOR_STATE_CW, MOTOR_STATE_CCW})
    public @interface MotorState {
    }

    private I2cDevice mI2cDevice;
    private DcMotor[] mMotors;
    private SteeringEngine mSteeringEngine;

    private PWM _pwm;

    public MotorHAT(String i2cBusName) throws IOException {
        this(i2cBusName, DEFAULT_I2C_ADDRESS);
    }

    public MotorHAT(String i2cBusName, int i2cAddress) throws IOException {
        PeripheralManager pioService = PeripheralManager.getInstance();
        I2cDevice device = pioService.openI2cDevice(i2cBusName, i2cAddress);
        try {
            mI2cDevice = device;
            _pwm = new PWM(mI2cDevice);
            _pwm.setPWMFreq(def_freq);
            initDCMotors();
            initmSteeringEngines();
        } catch (IOException | RuntimeException e) {
            try {
                close();
            } catch (IOException | RuntimeException ignored) {
            }
            throw e;
        }
    }


    /**
     * 初始化直流电机组
     */
    private void initDCMotors() throws IOException {
        mMotors = new DcMotor[MAX_DC_MOTORS];
        mMotors[0] = new DcMotor(8, 9, 10);
        mMotors[1] = new DcMotor(13, 12, 11);
        mMotors[2] = new DcMotor(2, 3, 4);
        mMotors[3] = new DcMotor(7, 6, 5);
    }


    /**
     * 初始化直流电机组
     */
    private void initmSteeringEngines() throws IOException {
        mSteeringEngine = new SteeringEngine("PWM0");
    }

    @Override
    public void close() throws IOException {
        if (mI2cDevice != null) {
            try {
                mI2cDevice.close();
            } finally {
                mI2cDevice = null;
            }
        }
    }


    class PWM {

        private static final int REG_MODE_1 = 0x00;
        private static final int REG_MODE_2 = 0x01;
        private static final int REG_PRESCALE = 0xFE;
        private static final int REG_LED_0_ON_L = 0x06;
        private static final int REG_LED_0_ON_H = 0x07;
        private static final int REG_LED_0_OFF_L = 0x08;
        private static final int REG_LED_0_OFF_H = 0x09;
        private static final int REG_ALL_LED_ON_L = 0xFA;
        private static final int REG_ALL_LED_ON_H = 0xFB;
        private static final int REG_ALL_LED_OFF_L = 0xFC;
        private static final int REG_ALL_LED_OFF_H = 0xFD;

        private static final byte ALLCALL = 0x01;
        private static final byte OUTDRV = 0x04;
        private static final byte SLEEP = 0x10;
        private static final byte RESTART = (byte) 0x80;

        private static final int DC_PIN_LOW = 0; //直流低电平
        private static final int DC_PIN_HIGH = 4096;//直流高电平

        private static final int MIN_SPEED = 0;//直流电机最小速度
        private static final int MAX_SPEED = 255;//直流电机最大速度

        private I2cDevice mI2cDevice;


        public PWM(I2cDevice mI2cDevice) throws IOException {
            this.mI2cDevice = mI2cDevice;
            initialize(mI2cDevice);
        }


        /**
         * 初始化设备
         *
         * @param device i2c 总线设备
         * @throws IOException
         */
        private void initialize(I2cDevice device) throws IOException {
            mI2cDevice = device;

            // reset
//            setAllPwm(0, 0);
            mI2cDevice.writeRegByte(REG_ALL_LED_ON_L, (byte) DC_PIN_LOW);
            mI2cDevice.writeRegByte(REG_ALL_LED_ON_H, (byte) DC_PIN_LOW);
            mI2cDevice.writeRegByte(REG_ALL_LED_OFF_L, (byte) DC_PIN_LOW);
            mI2cDevice.writeRegByte(REG_ALL_LED_OFF_H, (byte) DC_PIN_LOW);

            mI2cDevice.writeRegByte(REG_MODE_2, OUTDRV);
            mI2cDevice.writeRegByte(REG_MODE_1, ALLCALL);
            SystemClock.sleep(5);

            byte mode1 = mI2cDevice.readRegByte(REG_MODE_1);
//            // Remove the restart and sleep bits.
            mode1 = (byte) (mode1 & ~(RESTART | SLEEP));
//            // Sleep while we set the prescale value.
            mI2cDevice.writeRegByte(REG_MODE_1, (byte) (mode1));
            SystemClock.sleep(5);

//            setPWMFreq(def_freq);

//            float prescaleval = 25000000f // 25MHz
//                    / 4096 // 12-bit
//                    / 1600 // motor frequency
//                    - 1; // pineapple
//            byte prescale = (byte) (prescaleval + 0.5f);
//            mI2cDevice.writeRegByte(REG_PRESCALE, prescale);
//
//            // Restart: clear the sleep bit, wait for the oscillator to stabilize, set the restart bit.
//            // https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf (page 15)
//            mI2cDevice.writeRegByte(REG_MODE_1, (byte) (mode1 & ~SLEEP));
//            try {
//                TimeUnit.MICROSECONDS.sleep(500);
//            } catch (InterruptedException ignored) {
//            }
//            mI2cDevice.writeRegByte(REG_MODE_1, (byte) (mode1 | RESTART));

        }


        public void setPWMFreq(float freq) {
            float prescaleval = 25000000f // 25MHz
                    / 4096 // 12-bit
                    / freq // motor frequency
                    - 1; // pineapple
            float prescale = (byte) (prescaleval + 0.5f);
            try {
                byte oldmode = mI2cDevice.readRegByte(REG_MODE_1);
                int newmode = (oldmode & 0x7F) | 0x10;            // sleep
                mI2cDevice.writeRegByte(REG_MODE_1, (byte) newmode);      // go to sleep
                mI2cDevice.writeRegByte(REG_PRESCALE, (byte) (Math.floor(prescale)));
                mI2cDevice.writeRegByte(REG_MODE_1, oldmode);
                SystemClock.sleep(5);
                mI2cDevice.writeRegByte(REG_MODE_1, (byte) (oldmode | 0x80));
            } catch (IOException e) {
                e.printStackTrace();
            }
        }

        /**
         * 设置指定针脚的值
         *
         * @throws IOException
         */
        private void setAllPwm(int on, int off) throws IOException {
            mI2cDevice.writeRegByte(REG_ALL_LED_ON_L, (byte) (on & 0xFF));
            mI2cDevice.writeRegByte(REG_ALL_LED_ON_H, (byte) (on >> 8));
            mI2cDevice.writeRegByte(REG_ALL_LED_OFF_L, (byte) (off & 0xFF));
            mI2cDevice.writeRegByte(REG_ALL_LED_OFF_H, (byte) (off >> 8));
        }

        /**
         * 设置指定针脚的值
         *
         * @throws IOException
         */
        private void setPwm(int channel, int on, int off) throws IOException {
            int offset = 4 * channel;
            mI2cDevice.writeRegByte(REG_LED_0_ON_L + offset, (byte) (on & 0xFF));
            mI2cDevice.writeRegByte(REG_LED_0_ON_H + offset, (byte) (on >> 8));
            mI2cDevice.writeRegByte(REG_LED_0_OFF_L + offset, (byte) (off & 0xFF));
            mI2cDevice.writeRegByte(REG_LED_0_OFF_H + offset, (byte) (off >> 8));
        }
    }


    /**
     * @param num
     * @return 直流电机对象 {@link DcMotor}
     */
    public DcMotor getMotor(@IntRange(from = 1, to = 4) int num) {
        if ((num < 1) || (num > 4)) {
            throw new IllegalArgumentException("电机编号必须在1~4之间");
        }
        return mMotors[num - 1];
    }

    /**
     * @param
     * @return 舵机对象 {@link DcMotor}
     */
    public SteeringEngine getSteeringEngine() {
        if (mSteeringEngine != null) {
            try {
                mSteeringEngine = new SteeringEngine("PWM0");
            } catch (IOException e) {
                e.printStackTrace();
            }
        }
        return mSteeringEngine;
    }


    /**
     * 直流电机扩展类
     */
    final class DcMotor {

        private final int mPwmPin, mIn1Pin, mIn2Pin;
        private int mSpeed;


        public DcMotor(int pwmPin, int in1Pin, int in2Pin) throws IOException {
            mPwmPin = pwmPin;
            mIn1Pin = in1Pin;
            mIn2Pin = in2Pin;
        }

        private int Current_State = MOTOR_STATE_RELEASE;

        public int getCurrent_State() {
            return Current_State;
        }

        /**
         * 设置指定针脚的值
         *
         * @param pin   针脚地址
         * @param value 高低电平
         * @throws IOException
         */
        private void setPin(int pin, boolean value) throws IOException {
            if (_pwm == null) {
                return;
            }
            _pwm.setPwm(pin, value ? PWM.DC_PIN_HIGH : PWM.DC_PIN_LOW, value ? PWM.DC_PIN_LOW : PWM.DC_PIN_HIGH);
        }


        /**
         * 设置动作
         *
         * @param state 动作 {@link MotorHAT.MotorState}
         * @throws IOException
         */
        public void commond(@MotorHAT.MotorState int state) throws IOException {
            if (state == Current_State) {
                return;
            }
            switch (state) {
                case MOTOR_STATE_RELEASE://制动
                    Current_State = MOTOR_STATE_RELEASE;
                    setPin(mIn1Pin, false);
                    setPin(mIn2Pin, false);
                    break;
                case MOTOR_STATE_CW://顺时针转
                    Current_State = MOTOR_STATE_CW;
                    setPin(mIn2Pin, false);
                    setPin(mIn1Pin, true);
                    break;
                case MOTOR_STATE_CCW://逆时针转动
                    Current_State = MOTOR_STATE_CCW;
                    setPin(mIn1Pin, false);
                    setPin(mIn2Pin, true);
                    break;
            }
        }

        /**
         * 设置速度
         *
         * @param speed 速度值 0~255
         * @throws IOException
         */
        public void setSpeed(int speed) throws IOException {
            if (_pwm == null) {
                return;
            }
            if (speed < PWM.MIN_SPEED) {
                speed = PWM.MIN_SPEED;
            } else if (speed > PWM.MAX_SPEED) {
                speed = PWM.MAX_SPEED;
            }
            if (mSpeed != speed) {
                mSpeed = speed;
                _pwm.setPwm(mPwmPin, 0, speed << 4);
            }
        }
    }
}
